use memory_addr::PhysAddr;
use tock_registers::interfaces::{ReadWriteable, Readable, Writeable};

use crate::mem::phys_to_virt;
use core::ptr::NonNull;

pub use super::regs::*;
pub use super::types::*;

unsafe impl Send for FCanController {}
unsafe impl Sync for FCanController {}

impl FCanController {
    pub fn new(base: *mut u8) -> Self {
        Self {
            base: NonNull::new(base).unwrap().cast(),
            is_ready: false,
            use_canfd: false,
            caps: 0,
        }
    }

    const fn regs(&self) -> &FCanRegs {
        unsafe { &self.base.as_ref().0 }
    }

    pub fn init(&mut self) -> Result<(), &'static str> {
        let regs = self.regs();

        // 清除控制寄存器，确保CAN控制器停止传输
        regs.ctrl.set(0);
        let reg_value = regs.ctrl.get();
        info!("Initial ctrl value: {:#x}", reg_value);

        // 检查是否成功进入配置模式（传输使能位应该为0）
        if (reg_value & 1) != 0 {
            // FCAN_CTRL_XFER_MASK = 0x1
            error!("CAN is not in configuration mode.");
            return Err("CAN is not in configuration mode");
        }

        // 重置CAN控制器
        regs.ctrl.modify(CTRL::RST::SET); // FCAN_CTRL_RST_MASK
        info!(
            "CAN controller reset initiated, ctrl: {:#x}",
            regs.ctrl.get()
        );

        self.is_ready = true;

        info!("CAN controller initialized successfully");
        Ok(())
    }

    pub fn set_canfd_enable(&mut self, enable: bool) {
        self.use_canfd = enable;
        if enable {
            // 设置CAN FD模式相关位
            self.regs().ctrl.modify(CTRL::FDCRC::SET);
            self.regs().ctrl.modify(CTRL::IOF::SET);
            info!("CAN FD mode enabled, ctrl: {:#x}", self.regs().ctrl.get());
        } else {
            self.regs().ctrl.modify(CTRL::FDCRC::CLEAR);
        }
    }

    // todo set_can_mode
    pub fn set_can_mode(&mut self, mode: u32) {
        info!("Setting CAN mode to {}", mode);
        self.regs().ctrl.modify(CTRL::TXREQ::SET);
        info!("CAN mode set, ctrl: {:#x}", self.regs().ctrl.get());
    }

    pub fn set_enable(&mut self, enable: bool) {
        if enable {
            self.regs().ctrl.modify(CTRL::XFER::SET);
            info!(
                "CAN controller enabled, ctrl: {:#x}",
                self.regs().ctrl.get()
            );
        } else {
            self.regs().ctrl.modify(CTRL::XFER::CLEAR);
            info!(
                "CAN controller disabled, ctrl: {:#x}",
                self.regs().ctrl.get()
            );
        }
    }

    pub fn set_baudrate(&mut self, config: &FCanBaudrateConfig) -> Result<(), &'static str> {
        if !self.is_ready {
            return Err("CAN controller not initialized");
        }

        let mut timing_config = *config;

        // 如果启用自动计算，先计算波特率参数
        if config.auto_calc {
            self.calc_bittiming(&mut timing_config)?;
        }

        // 设置时序参数，参考C代码FCanSetTiming
        self.set_timing(&timing_config)?;

        Ok(())
    }

    /// 设置CAN时序参数，参考C代码FCanSetTiming
    fn set_timing(&mut self, config: &FCanBaudrateConfig) -> Result<(), &'static str> {
        // 暂时禁用传输，如C代码所示
        let transfer_enable = (self.regs().ctrl.get() & 1) != 0;
        if transfer_enable {
            self.regs().ctrl.modify(CTRL::XFER::CLEAR);
        }
        info!("Setting timing, ctrl before: {:#x}", self.regs().ctrl.get());

        // 构建寄存器值，按照C代码的位布局
        let reg_val = ((config.brp.saturating_sub(1)) << 16)
            | ((config.prop_seg.saturating_sub(1)) << 2)
            | ((config.phase_seg1.saturating_sub(1)) << 5)
            | ((config.phase_seg2.saturating_sub(1)) << 8)
            | (config.sjw.saturating_sub(1));
        info!(
            "Setting timing: brp={}, prop_seg={}, phase_seg1={}, phase_seg2={}, sjw={}",
            config.brp, config.prop_seg, config.phase_seg1, config.phase_seg2, config.sjw
        );

        if reg_val == 0 {
            return Err("Invalid timing configuration");
        }

        // 根据段类型写入对应寄存器
        match config.segment {
            FCanSegmentType::Arbitration => {
                // 直接写入完整寄存器值，而不是使用字段组合
                self.regs().arb_rate_ctrl.set(reg_val);
                info!(
                    "Arbitration timing set, arb_rate_ctrl: {:#x}",
                    self.regs().arb_rate_ctrl.get()
                );
            }
            FCanSegmentType::Data => {
                self.regs().dat_rate_ctrl.set(reg_val);
                info!(
                    "Data timing set, dat_rate_ctrl: {:#x}",
                    self.regs().dat_rate_ctrl.get()
                );
            }
        }
        /*
                // 恢复传输使能状态
                if transfer_enable {
                    self.regs().ctrl.modify(CTRL::XFER::SET);
                }
        */
        Ok(())
    }

    /// 自动计算波特率参数，完整版本的FCanCalcBittiming
    fn calc_bittiming(&self, config: &mut FCanBaudrateConfig) -> Result<(), &'static str> {
        const FCAN_CLK_FREQ_HZ: u32 = 200000000;
        const CAN_CALC_SYNC_SEG: u32 = 1;
        const CAN_CALC_MAX_ERROR: u32 = 50; // 最大误差5%

        let btc = if config.segment == FCanSegmentType::Arbitration {
            &FCAN_ARB_BIT_CONST
        } else {
            &FCAN_DATA_BIT_CONST
        };

        let target_baudrate = config.baudrate;
        let sample_point_nominal = if config.sample_point != 0 {
            config.sample_point
        } else {
            // 根据波特率选择采样点
            if target_baudrate > 4_000_000 {
                650
            } else if target_baudrate > 2_000_000 {
                680
            } else if target_baudrate > 1_000_000 {
                725
            } else if target_baudrate > 800_000 {
                750
            } else if target_baudrate > 500_000 {
                800
            } else {
                875
            }
        };

        let mut best_baudrate_error = u32::MAX;
        let mut best_sample_point_error = u32::MAX;
        let mut best_tseg = 0;
        let mut best_brp = 0;

        // 遍历所有可能的tseg组合，从最大到最小
        for tseg in
            ((btc.tseg1_min + btc.tseg2_min) * 2..=(btc.tseg1_max + btc.tseg2_max) * 2 + 1).rev()
        {
            let tsegall = CAN_CALC_SYNC_SEG + tseg / 2;

            // 计算BRP值，包括tseg余数补偿
            let mut brp = FCAN_CLK_FREQ_HZ / (tsegall * target_baudrate) + (tseg % 2);

            // 选择系统可能的BRP步长
            brp = (brp / btc.brp_inc) * btc.brp_inc;

            // 检查BRP范围
            if brp < btc.brp_min || brp > btc.brp_max {
                continue;
            }

            // 计算实际波特率和误差
            let baudrate = FCAN_CLK_FREQ_HZ / (brp * tsegall);
            let baudrate_error = if target_baudrate > baudrate {
                target_baudrate - baudrate
            } else {
                baudrate - target_baudrate
            };

            // 如果波特率误差更大，跳过
            if baudrate_error > best_baudrate_error {
                continue;
            }

            // 如果找到更好的波特率，重置采样点误差
            if baudrate_error < best_baudrate_error {
                best_sample_point_error = u32::MAX;
            }

            // 计算采样点参数
            let (_tseg1, _tseg2, sample_point_error) =
                self.update_sample_point(btc, sample_point_nominal, tseg / 2);

            // 检查采样点误差
            if sample_point_error > best_sample_point_error {
                continue;
            }

            // 更新最佳参数
            best_sample_point_error = sample_point_error;
            best_baudrate_error = baudrate_error;
            best_tseg = tseg / 2;
            best_brp = brp;

            // 如果找到完美匹配，提前退出
            if baudrate_error == 0 && sample_point_error == 0 {
                break;
            }
        }

        // 检查波特率误差是否在可接受范围内
        if best_baudrate_error != 0 {
            let error_permille = (best_baudrate_error as u64 * 1000) / target_baudrate as u64;
            if error_permille > CAN_CALC_MAX_ERROR as u64 {
                return Err("Baudrate error too large");
            }
        }

        // 计算最终的时序参数
        let (tseg1, tseg2, _) = self.update_sample_point(btc, sample_point_nominal, best_tseg);

        config.prop_seg = tseg1 / 2;
        config.phase_seg1 = tseg1 - config.prop_seg;
        config.phase_seg2 = tseg2;
        config.brp = best_brp;

        // 设置SJW参数
        config.sjw = if config.sjw == 0 || btc.sjw_max == 0 {
            1
        } else {
            let mut sjw = config.sjw;
            if sjw > btc.sjw_max {
                sjw = btc.sjw_max;
            }
            if tseg2 < sjw {
                sjw = tseg2;
            }
            sjw
        };

        // 验证最终波特率计算
        let final_baudrate = FCAN_CLK_FREQ_HZ / (config.brp * (CAN_CALC_SYNC_SEG + tseg1 + tseg2));
        if final_baudrate != target_baudrate {
            info!(
                "Calculated baudrate {} differs from target {}",
                final_baudrate, target_baudrate
            );
        }

        info!(
            "Calculated timing: brp={}, prop_seg={}, phase_seg1={}, phase_seg2={}, sjw={}",
            config.brp, config.prop_seg, config.phase_seg1, config.phase_seg2, config.sjw
        );

        Ok(())
    }

    /// 更新采样点参数，对应C代码中的FCanUpdateSamplePoint
    fn update_sample_point(
        &self,
        btc: &FCanBitimingConst,
        sample_point_nominal: u32,
        tseg: u32,
    ) -> (u32, u32, u32) {
        let mut best_tseg1 = 0;
        let mut best_tseg2 = 0;
        let mut best_error = u32::MAX;

        // 遍历所有可能的tseg1和tseg2组合
        for tseg1 in btc.tseg1_min..=btc.tseg1_max.min(tseg) {
            let tseg2 = tseg - tseg1;

            if tseg2 < btc.tseg2_min || tseg2 > btc.tseg2_max {
                continue;
            }

            // 计算当前采样点 (以千分之一为单位)
            let current_sample_point = ((1 + tseg1) * 1000) / (1 + tseg1 + tseg2);

            // 计算采样点误差
            let error = if current_sample_point > sample_point_nominal {
                current_sample_point - sample_point_nominal
            } else {
                sample_point_nominal - current_sample_point
            };

            if error < best_error {
                best_error = error;
                best_tseg1 = tseg1;
                best_tseg2 = tseg2;
            }
        }

        (best_tseg1, best_tseg2, best_error)
    }

    /// 将DLC值转换为实际数据长度，模拟FCanGetDlcLen函数
    fn get_dlc_len(&self, dlc: u32) -> u8 {
        const DLC_TABLE: [u8; 16] = [0, 1, 2, 3, 4, 5, 6, 7, 8, 12, 16, 20, 24, 32, 48, 64];
        if dlc < 16 { DLC_TABLE[dlc as usize] } else { 0 }
    }

    /// 将数据长度转换为DLC值，模拟FCanSetDlcLen函数
    fn set_dlc_len(&self, dlc_len: u8) -> u8 {
        const DLC_TABLE: [u8; 16] = [0, 1, 2, 3, 4, 5, 6, 7, 8, 12, 16, 20, 24, 32, 48, 64];
        for i in 0..16 {
            if DLC_TABLE[i] == dlc_len {
                return i as u8;
            }
        }
        0
    }

    /// 将CAN帧转换为发送缓冲区格式，模拟C代码中的FCanFrameToBuf
    fn frame_to_buf(&self, frame: &FCanFrame, buf: &mut [u32]) -> usize {
        let mut len = 0;

        // 获取发送数据长度
        let can_send_dlc = self.set_dlc_len(frame.candlc);
        info!(
            "CAN send DLC: {}, original candlc: {}",
            can_send_dlc, frame.candlc
        );

        // 标准帧数据转换为dlc
        // id - 使用正确的FCAN_IDR_ID1_SET宏等效操作
        let mut id = (frame.canid & CAN_SFF_MASK) << FCAN_IDR_ID1_SHIFT; // FCAN_IDR_ID1_SET
        if (frame.canid & CAN_RTR_FLAG) != 0 {
            id |= FCAN_IDR_SRR_MASK;
        }

        // dlc
        // CAN-FD协议 标准帧
        if self.use_canfd {
            info!("enter if (instance_p->use_canfd == TRUE)");

            let mut dlc = ((can_send_dlc as u32) << FCANFD_IDR1_SDLC_SHIFT) | FTCANFD_IDR_PAD_MASK;
            dlc |= FTCANFD_ID1_FDL_MASK;

            let mut frame_flags = frame.flags;
            frame_flags |= CANFD_BRS;

            if (frame_flags & CANFD_BRS) != 0 {
                dlc |= FTCANFD_ID1_BRS_MASK;
            }
            if (frame_flags & CANFD_ESI) != 0 {
                dlc |= FTCANFD_ID1_ESI_MASK;
            }

            id |= dlc;
            buf[len] = id.swap_bytes(); // be32_to_cpu
            len += 1;
        }

        // 数据帧
        if (frame.canid & CAN_RTR_FLAG) == 0 {
            info!("enter if ( !(frame_p->canid & CAN_RTR_FLAG) )");
            let mut j = 0;
            let mut i = frame.candlc as i32;

            while i > 0 {
                // 按32位读取数据，模拟 *(u32 *)(frame_p->data + j)
                let mut data_word: u32 = 0;
                for byte_offset in 0..4 {
                    if j + byte_offset < frame.data.len() && byte_offset < i as usize {
                        data_word |= (frame.data[j + byte_offset] as u32) << (byte_offset * 8);
                    }
                }
                buf[len] = data_word;
                len += 1;
                j += 4;
                i -= 4;
            }
        }

        len // 返回实际使用的缓冲区长度
    }

    pub fn send_frame(&mut self, frame: &FCanFrame) -> Result<(), &'static str> {
        if !self.is_ready {
            return Err("CAN controller not initialized");
        }

        // 使用正确的FIFO满检查逻辑
        if self.is_tx_fifo_full() {
            info!("TX FIFO is full, cannot send frame");
            return Err("TX FIFO is full");
        }

        let fifo_cnt = self.regs().fifo_cnt.get();
        info!("CAN TX FIFO count: {:#x}", fifo_cnt);

        // 按照C代码的实现，使用FCanFrameToBuf将帧转换为缓冲区，然后逐个写入TX FIFO
        let mut buf = [0u32; (FCAN_DATA_LENGTH / 4) + 2];
        let len = self.frame_to_buf(frame, &mut buf);
        info!("Prepared CAN frame for sending, buffer length: {}", len);

        for i in 0..len {
            // 像C代码一样，所有数据都写入同一个TX FIFO寄存器
            // 硬件会自动管理FIFO指针递增
            self.regs().tx_fifo.set(buf[i]);
            info!("Sent word {}: 0x{:08X}", i, buf[i]);
        }

        // self.regs().ctrl.modify(CTRL::TXREQ::SET);
        info!(
            "Sent CAN frame with ID: {:X}, DLC: {}, Flags: {}",
            frame.canid, frame.candlc, frame.flags
        );

        Ok(())
    }

    pub fn recv_frame(&mut self, frame: &mut FCanFrame) -> Result<(), &'static str> {
        if !self.is_ready {
            return Err("CAN controller not initialized");
        }

        // 使用正确的FIFO状态检查逻辑
        if self.is_rx_fifo_empty() {
            return Err("RX FIFO empty");
        }

        // 清空帧数据 - 对应C代码中的memset(frame_p, 0, sizeof(FCanFrame))
        *frame = FCanFrame::new();

        // 读取第一个字（帧ID和控制信息） - 对应FCAN_READ_REG32(base_address, FCAN_RX_FIFO_OFFSET)
        let canid = self.regs().rx_fifo.get();
        // if canid is big-endian, use swap change to little-endian
        let canid = canid.swap_bytes(); // be32_to_cpu

        let dlc: u32;

        // 检查是否为CANFD帧
        if canid & FTCANFD_ID1_FDL_MASK != 0 {
            info!("enter canid & FTCANFD_ID1_FDL_MASK");

            if canid & FTCANFD_ID1_BRS_MASK != 0 {
                frame.flags |= CANFD_BRS;
            }

            if canid & FTCANFD_ID1_ESI_MASK != 0 {
                frame.flags |= CANFD_ESI;
            }

            // dlc = FTCANFD_ID1_SDLC_GET(canid); - 对应GET_REG32_BITS((canid),14, 11)
            dlc = (canid >> 11) & 0xF;
        } else {
            // dlc = FCAN_IDR_DLC_GET(canid); - 对应GET_REG32_BITS((canid), 17, 14)
            dlc = (canid >> 14) & 0xF;
        }

        // The received frame is a standard format frame
        // frame_p->canid = FCAN_IDR_ID1_GET(canid); - 对应GET_REG32_BITS((canid), 31, 21)
        frame.canid = (canid >> FCAN_IDR_ID1_SHIFT) & CAN_SFF_MASK;

        if canid & FCAN_IDR_SRR_MASK != 0 {
            info!("enter if (canid & FCAN_IDR_SRR_MASK)");
            frame.canid |= CAN_RTR_FLAG;
        }

        // frame_p->candlc = FCanGetDlcLen(dlc);
        frame.candlc = self.get_dlc_len(dlc);

        // 如果不是RTR帧，读取数据
        if (frame.canid & CAN_RTR_FLAG) == 0 {
            let mut j = 0;
            let mut i = frame.candlc as i32;

            // 对应C代码: for (i = frame_p->candlc; i > 0; i -= 4)
            while i > 0 {
                // *(u32 *)(frame_p->data + j) = FCAN_READ_REG32(base_address, FCAN_RX_FIFO_OFFSET);
                let data_word = self.regs().rx_fifo.get();

                // 将32位数据按字节存储到帧数据中
                for byte_idx in 0..4 {
                    if j + byte_idx < frame.data.len() && byte_idx < i as usize {
                        frame.data[j + byte_idx] = ((data_word >> (byte_idx * 8)) & 0xFF) as u8;
                    }
                }

                j += 4;
                i -= 4;
            }

            // 对应C代码: if (i > 0) { *(u32 *)(frame_p->data + j) = FCAN_READ_REG32(...); }
            // 注意：在while循环结束时，如果i > 0说明还有剩余数据需要读取
            // 但在我们的实现中，这种情况已经在while循环中处理了
        }

        Ok(()) // 对应return FCAN_SUCCESS;
    }

    /// 设置ID掩码过滤器，支持标准/扩展帧
    pub fn set_id_mask_filter(&mut self, config: &FCanIdMaskConfig) -> Result<(), &'static str> {
        if config.filter_index > 3 {
            error!("Invalid filter index");
            return Err("Invalid filter index");
        }

        // 标准帧需要左移18位 - 使用FCAN_ACC_IDN_SHIFT
        let (id, mask) = match config.frame_type {
            FCanFrameType::Standard => (config.id << 18, config.mask << 18), // FCAN_ACC_IDN_SHIFT
            _ => (config.id, config.mask),
        };

        match config.filter_index {
            0 => {
                self.regs().acc_id0.set(id);
                self.regs().acc_id0_mask.set(mask);
            }
            1 => {
                self.regs().acc_id1.set(id);
                self.regs().acc_id1_mask.set(mask);
            }
            2 => {
                self.regs().acc_id2.set(id);
                self.regs().acc_id2_mask.set(mask);
            }
            3 => {
                self.regs().acc_id3.set(id);
                self.regs().acc_id3_mask.set(mask);
            }
            _ => return Err("Invalid filter index"),
        }

        Ok(())
    }

    pub fn enable_id_mask_filter(&mut self) {
        self.regs().ctrl.modify(CTRL::AIME::SET);
    }

    pub fn disable_id_mask_filter(&mut self) {
        self.regs().ctrl.modify(CTRL::AIME::CLEAR);
    }

    pub fn is_rx_fifo_empty(&self) -> bool {
        let fifo_cnt = self.regs().fifo_cnt.get();
        // 对应C代码: FCAN_RX_FIFO_EMPTY(base_addr) (0 == FCAN_FIFO_CNT_RFN_GET(...))
        fcan_fifo_cnt_rfn_get(fifo_cnt) == 0
    }

    pub fn is_tx_fifo_full(&self) -> bool {
        let fifo_cnt = self.regs().fifo_cnt.get();
        let tfn = fcan_fifo_cnt_tfn_get(fifo_cnt);

        // 对应C代码宏的逻辑
        if self.use_canfd {
            // FCANFD_TX_FIFO_FULL: ((CAN_FIFO_BYTE_LEN) - (4 * tfn)) < KEEP_CANFD_FIFO_MIN_LEN
            (CAN_FIFO_BYTE_LEN - (4 * tfn)) < KEEP_CANFD_FIFO_MIN_LEN
        } else {
            // FCAN_TX_FIFO_FULL: ((CAN_FIFO_BYTE_LEN) - (4 * tfn)) < KEEP_CAN_FIFO_MIN_LEN
            (CAN_FIFO_BYTE_LEN - (4 * tfn)) < KEEP_CAN_FIFO_MIN_LEN
        }
    }

    pub fn get_error_counters(&self) -> u32 {
        self.regs().err_cnt.get()
    }

    pub fn deinit(&mut self) {
        self.regs().ctrl.write(CTRL::XFER::CLEAR);
        self.is_ready = false;
    }

    pub fn send_test_frame(&mut self, standard: bool) -> Result<(), &'static str> {
        const FCAN_SEND_ID: u32 = 0x123;
        const FCAN_SEND_LENGTH: u8 = 16;

        let mut frame = FCanFrame::new();
        frame.canid = FCAN_SEND_ID;
        if standard {
            frame.canid &= CAN_SFF_MASK; // Standard frame format
        } else {
            frame.canid |= CAN_EFF_FLAG; // Extended frame format
        }
        frame.candlc = FCAN_SEND_LENGTH;

        // frame.flags = if self.use_canfd { CANFD_BRS } else { 0 };
        info!("can send id is {}", frame.canid);
        info!("can send dlc is {}", frame.candlc);
        info!("can send data is ");
        // Fill test data
        for i in 0..frame.candlc {
            frame.data[i as usize] = i as u8;
            info!("data[{}] = {}", i, frame.data[i as usize]);
        }

        self.send_frame(&frame)
    }

    pub fn receive_frames(&mut self) -> Result<(), &'static str> {
        let mut timeout = 100;
        let mut frame = FCanFrame::new();

        // 等待RX FIFO有数据
        while self.is_rx_fifo_empty() && timeout > 0 {
            timeout -= 1;
            // 简单的延时等待
            for _i in 0..1000000 {
                unsafe {
                    core::ptr::read_volatile(&0u32);
                }
            }
        }

        if timeout == 0 {
            error!("Timeout waiting for CAN frame");
            return Ok(());
        }

        // 接收帧数据
        match self.recv_frame(&mut frame) {
            Ok(_) => {
                info!("CAN recv id is {:#x}", frame.canid);
                info!("CAN recv dlc is {}", frame.candlc);
                info!(
                    "CAN recv data is: {:02x?}",
                    &frame.data[..frame.candlc as usize]
                );
            }
            Err(e) => {
                error!("Failed to receive frame: {}", e);
            }
        }

        Ok(())
    }
}

impl FCanSystem {
    pub fn new() -> Self {
        const CONTROLLER_BASES: [PhysAddr; FCAN_CONTROLLERS] = [
            (pa!(0x2800A000)), // fcan0 根据实际地址修改
            (pa!(0x2800B000)), // fcan1 根据实际地址修改
        ];

        let controllers =
            CONTROLLER_BASES.map(|base| FCanController::new(phys_to_virt(base).as_mut_ptr()));
        Self { controllers }
    }

    pub fn controller(&mut self, index: usize) -> Option<&mut FCanController> {
        info!("Getting FCAN controller {}", index);
        if index < FCAN_CONTROLLERS {
            Some(&mut self.controllers[index])
        } else {
            None
        }
    }
}
